﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using GRIP.Cuda;
using Microsoft.Kinect;
using System.Windows.Interop;
using GRIP;

namespace GuiTest
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        KinectSensor sensor;
        byte[] colorFrame = new byte[640 * 480 * 4];
        CudaSource source;
        CudaHostBuffer ColorFrameHost;
        CudaHostBuffer DepthFrameHost;
        CudaDeviceBuffer ColorFrameDevice;
        CudaDeviceBuffer ColorFrameDevice2;
        CudaDeviceBuffer DepthFrameDevice;
        CudaDeviceBuffer IndexMapDevice;

        public MainWindow()
        {
            InitializeComponent();
            CudaManager.InitializeCuda();
            ColorFrameHost = new CudaHostBuffer(640, 480, 4);
            DepthFrameHost = new CudaHostBuffer(640, 480, sizeof(short));
            ColorFrameDevice = new CudaDeviceBuffer(640, 480, 4);
            ColorFrameDevice2 = new CudaDeviceBuffer(640, 480, 4);
            DepthFrameDevice = new CudaDeviceBuffer(640, 480, sizeof(short));
            IndexMapDevice = new CudaDeviceBuffer(640, 480, sizeof(int));
            source = new CudaSource(640, 480);

            sensor = KinectSensor.KinectSensors.First();
            sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
            sensor.SkeletonStream.Enable();
            sensor.Start();

            int[] indices = new int[307200];
            short[] fakeDepth = new short[307200];
            for (int i = 0; i < 307200; i++)
                fakeDepth[i] = 9600;
            ColorImagePoint[] points = new ColorImagePoint[307200];

            sensor.MapDepthFrameToColorFrame(DepthImageFormat.Resolution640x480Fps30, fakeDepth, ColorImageFormat.RgbResolution640x480Fps30, points);

            for (int i = 0; i < points.Length; i++)
            {
                ColorImagePoint point = points[i];
                int index = point.Y * 640 + point.X;
                if (index < 0 || index > 307199)
                    indices[i] = 0;
                else
                    indices[i] = index;
            }

            CudaManager.CopyBytesToDevice(indices, IndexMapDevice);
            GripGpu.BindSourceToKernel(source);

            image1.Source = source;
            sensor.AllFramesReady += sensor_AllFramesReady;
            CompositionTarget.Rendering += Render;
        }

        void sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            using (SkeletonFrame skelFrame = e.OpenSkeletonFrame())
            {
                using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
                {
                    using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
                    {
                        if (colorFrame == null || depthFrame == null)
                            return;

                        colorFrame.CopyPixelDataTo(ColorFrameHost.Data(), colorFrame.PixelDataLength);
                        depthFrame.CopyPixelDataTo(DepthFrameHost.Data(), depthFrame.PixelDataLength);
                        ColorFrameHost.CopyToDevice(ColorFrameDevice);
                        DepthFrameHost.CopyToDevice(DepthFrameDevice);

                        GripGpu.AlignColorFrame(ColorFrameDevice, ColorFrameDevice2, IndexMapDevice);
                        GripGpu.BackgroundSubtract(DepthFrameDevice, ColorFrameDevice2, 1);
                    }
                }
            }
        }

        TimeSpan _lastRender;
        void Render(object sender, EventArgs e)
        {
            source.Update();
        }

        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            sensor.AllFramesReady -= sensor_AllFramesReady;
            sensor.Stop();
            sensor.Dispose();
            //source.Dispose();
        }
    }
}
